#include <REGX52.H>
#include "motor.h"
#include "delay.h"
#include "hc04.h"
#include "OLED.h"
#include "sg90.h"

/*#define BZ 1
#define XJ 2
#define GS 3*/

sbit A25 = P1^7;
sbit A26 = P3^0;
sbit A27 = P3^1;

sbit trace_left  = P2^7;
sbit trace_right = P2^6;

sbit follow_left  = P2^5;
sbit follow_right = P2^4;
sbit D5 = P3^7; //设备变量led1指向P3组IO口的第7口
sbit D6 = P3^6; //设备变量led2指向P3组IO口的第6口

extern Trig;
extern Echo;

int middle_dis;
int left_dis;
int right_dis;





void car_trace()
{
    if(trace_left == 0 && trace_right == 0){
        setMotorSpeeds(60, 60, 1, 1);
    }
    else if(trace_left == 1 && trace_right == 0){
        setMotorSpeeds(10, 80, 1, 1);
		
    }
    else if(trace_left == 0 && trace_right == 1){
        setMotorSpeeds(80, 10, 1, 1);
		
    }
    else {
        setMotorSpeeds(0, 0, 1, 1);
    }
}

void car_follow()
{
    if(follow_left == 0 && follow_right == 1){
        setMotorSpeeds(80, 30, 1, 1);
    }
    else if(follow_left == 1 && follow_right == 0){
        setMotorSpeeds(30, 80, 1, 1);
    }
    else if(follow_left == 0 && follow_right == 0){
        setMotorSpeeds(70, 70, 1, 1);
    }
    else {
        setMotorSpeeds(0, 0, 1, 1);
    }
}

void car_elude()
{
    middle_dis = get_distance();
    if(middle_dis > 35){
        setMotorSpeeds(100, 100, 1, 1);
    }
    else if(middle_dis < 10){
        setMotorSpeeds(100, 100, 0, 0);
        Delay150ms();
        setMotorSpeeds(0, 0, 1, 1);
    }
    else {
        setMotorSpeeds(0, 0, 1, 1);
        sg_left();
        Delay450ms();
        left_dis = get_distance();
        
        sg_middle();
        Delay450ms();
        
        sg_right();
        Delay450ms();
        right_dis = get_distance();
        
        sg_middle();
        Delay450ms();
        
        if(left_dis < right_dis){
            setMotorSpeeds(90, 60, 1, 1);
            Delay150ms();
            setMotorSpeeds(0, 0, 1, 1);
        }
        else {
            setMotorSpeeds(60, 90, 1, 1);
            Delay150ms();
            setMotorSpeeds(0, 0, 1, 1);
        }
    }
}

void runMotors()
{
    while(1)
    {
        if(A25 == 0 && A26 == 1 && A27 == 1){
            car_trace();
        }
        else if(A25 == 1 && A26 == 0 && A27 == 1){
            car_follow();
        }
        else if(A25 == 1 && A26 == 1 && A27 == 0){
            car_elude();
        }
    }
}

void main()
{
    Delay1000ms();
    InitTime0();
    Time1Init();

    sg_middle();
    Delay450ms();
    OLED_Init();
    OLED_Clear();
    Oled_Show_Str(2, 2, "-----ready-----");
    
    runMotors();  // 启动电机控制循环
}
